/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *********************************************************************/
#include <costmap_2d/observation_buffer.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <sensor_msgs/point_cloud2_iterator.h>

using namespace std;
using namespace tf2;

namespace costmap_2d
{
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
                                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
                                     double raytrace_range, tf2_ros::Buffer &tf2_buffer, string global_frame,
                                     string sensor_frame, double tf_tolerance) :
        tf2_buffer_(tf2_buffer), tf_tolerance_(tf_tolerance), last_updated_(ros::Time::now()),
        observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
        global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
        min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
        obstacle_range_(obstacle_range), raytrace_range_(raytrace_range) {}

ObservationBuffer::~ObservationBuffer() {}

bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
    ros::Time transform_time = ros::Time::now();
    std::string tf_error;
    geometry_msgs::TransformStamped transformStamped;
    if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time,
            ros::Duration(tf_tolerance_), &tf_error))
    {
        ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
                  global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
        return false;
    }
    list<Observation>::iterator obs_it;
    for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
    {
        try
        {
            Observation &obs = *obs_it;
            geometry_msgs::PointStamped origin;
            origin.header.frame_id = global_frame_;
            origin.header.stamp = transform_time;
            origin.point = obs.origin_;
            // we need to transform the origin of the observation to the new global frame
            tf2_buffer_.transform(origin, origin, new_global_frame);
            obs.origin_ = origin.point;
            // we also need to transform the cloud of the observation to the new global frame
            tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
        }
        catch (TransformException &ex)
        {
            ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
                      new_global_frame.c_str(), ex.what());
            return false;
        }
    }
    // now we need to update our global_frame member
    global_frame_ = new_global_frame;
    return true;
}

void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2 &cloud)
{
    geometry_msgs::PointStamped global_origin;
    // create a new observation on the list to be populated
    // Step 1:在要填充的列表上创建新观测值
    observation_list_.push_front(Observation());
    // check whether the origin frame has been set explicitly or whether we should get it from the cloud
    // Step 2:检测原点frame是否已显式设置，或者我们是否应该从云中获取它。
    string origin_frame = sensor_frame_.empty() ? cloud.header.frame_id : sensor_frame_;
    try
    {
        // given these observations come from sensors... we'll need to store the origin pt of the sensor
        // Step 3: 鉴于这些观察结果来自传感器...我们需要存储传感器的原点
        geometry_msgs::PointStamped local_origin;
        local_origin.header.stamp = cloud.header.stamp;
        local_origin.header.frame_id = origin_frame;
        local_origin.point.x = 0;
        local_origin.point.y = 0;
        local_origin.point.z = 0;
        tf2_buffer_.transform(local_origin, global_origin, global_frame_);
        // 将 geometry_msgs::PointStamped 类型转换为 geometry_msgs::Point
        tf2::convert(global_origin.point, observation_list_.front().origin_);
        // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
        // Step 4: 确保将观测缓冲区的射线扫描障碍物范围传递给观测值
        observation_list_.front().raytrace_range_ = raytrace_range_;
        observation_list_.front().obstacle_range_ = obstacle_range_;
        sensor_msgs::PointCloud2 global_frame_cloud;
        // Step 5: transform the point cloud 转换点云
        tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
        global_frame_cloud.header.stamp = cloud.header.stamp;
        // now we need to remove observations from the cloud that are below or above our height thresholds
        // Step 6: 现在我们需要从云中删除低于或高于我们高度阈值的观测值
        sensor_msgs::PointCloud2 &observation_cloud = *(observation_list_.front().cloud_);
        observation_cloud.height = global_frame_cloud.height;
        observation_cloud.width = global_frame_cloud.width;
        observation_cloud.fields = global_frame_cloud.fields;
        observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
        observation_cloud.point_step = global_frame_cloud.point_step;
        observation_cloud.row_step = global_frame_cloud.row_step;
        observation_cloud.is_dense = global_frame_cloud.is_dense;
        unsigned int cloud_size = global_frame_cloud.height * global_frame_cloud.width;
        // 允许像容器一样修改sensor_msgs::PointCloud2
        sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
        modifier.resize(cloud_size);
        unsigned int point_count = 0;
        // copy over the points that are within our height bounds
        // Step 7: 复制高度范围内的点
        sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
        auto iter_global = global_frame_cloud.data.begin();
        auto iter_global_end = global_frame_cloud.data.end();
        auto iter_obs = observation_cloud.data.begin();
        for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
        {
            if ((*iter_z) <= max_obstacle_height_ && (*iter_z) >= min_obstacle_height_)
            {
                std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
                iter_obs += global_frame_cloud.point_step;
                ++point_count;
            }
        }
        // resize the cloud for the number of legal points
        // Step 8: 调整云的大小以达到法定点数
        modifier.resize(point_count);
        observation_cloud.header.stamp = cloud.header.stamp;
        observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
    }
    catch (TransformException &ex)
    {
        // if an exception occurs, we need to remove the empty observation from the list
        // Step 9: 如果发生异常，我们需要从列表中删除空观测值
        observation_list_.pop_front();
        ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s",
                  sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what());
        return;
    }
    // if the update was successful, we want to update the last updated time
    // Step 10: 如果更新成功，我们希望更新上次更新时间
    last_updated_ = ros::Time::now();
    // Step 11: we'll also remove any stale observations from the list
    purgeStaleObservations();
}

// returns a copy of the observations
void ObservationBuffer::getObservations(vector<Observation> &observations)
{
    // first... let's make sure that we don't have any stale observations
    // 首先，我们要确保没有任何陈旧的观察结果
    purgeStaleObservations();
    // now we'll just copy the observations for the caller
    // 现在我们为调用者复制观察结果
    for (auto & obs_it : observation_list_) observations.push_back(obs_it);
}

void ObservationBuffer::purgeStaleObservations()
{
    if (!observation_list_.empty())
    {
        // if we're keeping observations for no time... then we'll only keep one observation
        // 如果我们没有时间观察……那我们只做一个观察
        if (observation_keep_time_ == ros::Duration(0.0))
        {
            observation_list_.erase(++observation_list_.begin(), observation_list_.end());
            return;
        }
        // otherwise... we'll have to loop through the observations to see which ones are stale
        // 否则……我们必须在观察中循环，看看哪些是过时的
        for (auto obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
        {
            // check if the observation is out of date... and if it is, remove it and those that follow from the list
            // 检查观察是否过期…如果是，就把它和后面的那些从列表中删除
            if ((last_updated_ - obs_it->cloud_->header.stamp) > observation_keep_time_)
            {
                observation_list_.erase(obs_it, observation_list_.end());
                return;
            }
        }
    }
}

bool ObservationBuffer::isCurrent() const
{
    if (expected_update_rate_ == ros::Duration(0.0)) return true;
    bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
    if (!current)
    {
        ROS_WARN("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
                topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
    }
    return current;
}

void ObservationBuffer::resetLastUpdated()
{
    last_updated_ = ros::Time::now();
}
}  // namespace costmap_2d

